import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument,ExecuteProcess
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution,PythonExpression

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare   

def generate_launch_description():
    world_sdf = LaunchConfiguration('world_sdf')
    px4_gz_download = LaunchConfiguration('px4_gz_download')
    bridge_config = LaunchConfiguration('bridge_config')

    world_sdf_launch_arg = DeclareLaunchArgument(
        'world_sdf',
        default_value= 'px4_default.sdf',
        description='Gazebo world SDF file'
    )

    px4_gz_download_launch_arg = DeclareLaunchArgument(
        'px4_gz_download',
        default_value= 'False',
        description='Download PX4 Firmware'
    )

    bridge_config_launch_arg = DeclareLaunchArgument(
        'bridge_config',
        default_value= 'ros_gz_bridge.yaml',
        description='Bridge configuration file'
    )
    
    # Setup project paths
    pkg_sls_launch_sim = get_package_share_directory('sls_launch_sim')


    # 运行 pkg_sls_launch_sim 包中的 gzstart 文件夹中的 simulation-gazebo.py
    #如果px4_gz_download的值为True，则添加--overwrite参数
    #如果px4_gz_download的值为False，则不添加--overwrite参数

    # 构建基础命令
    base_cmd = [
        'python3',
        os.path.join(pkg_sls_launch_sim, 'gzstart', 'simulation-gazebo.py'),
        '--world', world_sdf,
        '--selfpath', pkg_sls_launch_sim,
    ]


    # 执行命令
    gz_sim_overwrite = ExecuteProcess(
        condition=IfCondition(PythonExpression(['True == ', px4_gz_download])),
        cmd=base_cmd+['--overwrite'],
        output='screen'
    )
    
    gz_sim = ExecuteProcess(
        condition=IfCondition(PythonExpression(['False == ', px4_gz_download])),
        cmd=base_cmd,
        output='screen'
    )
    #Bridge ROS topics and Gazebo messages for establishing communication
    bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        parameters=[{
            'config_file': PathJoinSubstitution([pkg_sls_launch_sim, 'config', bridge_config]),
            'qos_overrides./tf_static.publisher.durability': 'transient_local',
        }],
        output='screen'
    )

    return LaunchDescription([
        world_sdf_launch_arg,
        px4_gz_download_launch_arg,
        bridge_config_launch_arg,
        gz_sim,
        gz_sim_overwrite,
        bridge,
    ])